Option Explicit

Public Sub MainModule()

	' Declare Local Variables For FSM
	Dim currState As Integer ' Current State
	Dim nextState As Integer ' Next State
	Dim prevState As Integer ' Previous State
	Dim right_direction As Integer	  ' temp vars to calc
	Dim left_direction As Integer	  ' the pwm signal
	Dim local_direction As Single	  ' used for our global direction
	
	' Constants for state changes
	Const S0 as Integer = 0  ' State: R/C Power
	Const S1 as Integer = 1  ' State: R/C Forward
	Const S2 as Integer = 2  ' State: R/C Reverse
	Const S3 as Integer = 3  ' State: GPS Power
	Const S4 as Integer = 4  ' State: GPS Forward
	Const S5 as Integer = 5  ' State: GPS Reverse

	' Initalize all states
	prevState = 0
	currState = 0
	nextState = 0
'pwmr = 50 + LocDir*RDir
'pwml = 50 + LocDir*LDir
	Do 
'The next line will tell us how far we are going to left or right  
		local_direction = CSng(fb_power) / CSng(4)                  
		Select Case (currState) 
		Case (S0)
			If (rc_activate) Then
'Debug.Print "The R/C is on"					
				If (forward) Then
					If (prevState = S2) Then
'Debug.Print "Were pausing"
						left_power = 5
						right_power = 5
						Call Sleep(2.0)
					End If
					nextState = S1
				ElseIf (reverse) Then 
					If (prevState = S1) Then
'Debug.Print "Were pausing"
						left_power = 5
						right_power = 5
						Call Sleep(2.0)
					End If
					nextState = S2
				Else
					If (right) Then
'Debug.Print "Stopped Right"
						left_power = 10
						right_power = 0
					ElseIf (left) Then
'Debug.Print "Stopped Left"
						left_power = 0
						right_power = 10						
					Else
'Debug.Print "Stopped"
						left_power = 5
						right_power = 5						
					End If
					nextState = S0
				End If
			Else	
				left_power = 5
				right_power = 5
				nextState = S3
			End If
		Case (S1)
			If (right) Then				
'Debug.Print "Forward Right"
				right_direction = 50
				If (lr_power = QTR_PWR) Then
					left_direction = 37
				ElseIf (lr_power = HALF_PWR) Then
					left_direction = 25
				ElseIf (lr_power = THR_QTR_PWR) Then
					left_direction = 13
				ElseIf (lr_power = FULL_PWR) Then
					left_direction = 0
				End If
			ElseIf (left) Then		
'Debug.Print "Forward Left"
				left_direction = 50
				If (lr_power = QTR_PWR) Then
					right_direction = 37
				ElseIf (lr_power = HALF_PWR) Then
					right_direction = 25
				ElseIf (lr_power = THR_QTR_PWR) Then
					right_direction = 13
				ElseIf (lr_power = FULL_PWR) Then
					right_direction = 0
				End If
			Else
'Debug.Print "Forward Straight"
				right_direction = 50
				left_direction = 50	
			End If
			left_power = CInt((CSng(50)+local_direction*CSng(left_direction))/CSng(10))
			right_power = CInt((CSng(50)+local_direction*CSng(right_direction))/CSng(10))
			If (forward = FALSE) Then
				nextState = S0
			Else
				nextState = S1
			End If
		Case (S2)
			local_direction = local_direction * CSng(-1)
			If (right) Then
'Debug.Print "Reverse Right"
				right_direction = 50
				If (lr_power = QTR_PWR) Then
					left_direction = 37
				ElseIf (lr_power = HALF_PWR) Then
					left_direction = 25
				ElseIf (lr_power = THR_QTR_PWR) Then
					left_direction = 13
				ElseIf (lr_power = FULL_PWR) Then
					left_direction = 0
				End If				
			ElseIf (left) Then
'Debug.Print "Reverse Left"
				left_direction = 50
				If (lr_power = QTR_PWR) Then
					right_direction = 37
				ElseIf (lr_power = HALF_PWR) Then
					right_direction = 25
				ElseIf (lr_power = THR_QTR_PWR) Then
					right_direction = 13
				ElseIf (lr_power = FULL_PWR) Then
					right_direction = 0
				End If				
			Else
'Debug.Print "Reverse Straight"
				right_direction = 50
				left_direction = 50				
			End If
			left_power = CInt((CSng(50)+local_direction*CSng(left_direction))/CSng(10))
			right_power = CInt((CSng(50)+local_direction*CSng(right_direction))/CSng(10))
			If (reverse = FALSE) Then
				nextState = S0
			Else
				nextState =  S2
			End If
		Case (S3)
			If (gps_activate) Then
				Debug.Print "GPS is on"
			Else
				nextState = S0
			End If
		Case (S4)
			'this is the future gps forward case
		Case (S5)
			'this is the future gps reverse case
		Case Else
			'If used this is an error state
		End Select
		prevState = currState
		currState = nextState
'Debug.Print CStr(left_power); ": Left Power" 
'Debug.Print CStr(right_power); ": Right Power"
		Call Sleep(0.25)
	Loop                     ' End of Infinite Loop
End Sub
